/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2022-2022. All rights reserved.
 * Description: jpg jpeg发送
 */

#ifndef IMAGE_SENDER_PCD_SENDER_H
#define IMAGE_SENDER_PCD_SENDER_H
#include <dirent.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <sys/time.h>
#include <sys/types.h>
#include <ctime>
#include <unistd.h>

#include <fstream>
#include <iostream>
#include <vector>

#include "yaml/haf_yaml.h"
#include "image_sender_base.h"
#include "ara/lidar/lidarserviceinterface_common.h"
#include "ara/lidar/lidarserviceinterface_skeleton.h"
#include "ara/lidar/impl_type_lidarpointcloud.h"
#include "ara/lidar/impl_type_lidarpointfield.h"
#include "util.h"

const int16_t X_IN_POINTCLOUD = 0;
const int16_t Y_IN_POINTCLOUD = 4;
const int16_t Z_IN_POINTCLOUD = 8;
const int16_t INTENSITY_IN_POINTCLOUD = 28;
const int16_t X_IN_POINTCLOUD_SIMPLE = 0;
const int16_t Y_IN_POINTCLOUD_SIMPLE = 2;
const int16_t Z_IN_POINTCLOUD_SIMPLE = 4;
const int16_t INTENSITY_IN_POINTCLOUD_SIMPLE = 10;
const int32_t CI13 = 13;
const int32_t CI100 = 100;

namespace ara {
namespace lidar {
constexpr ara::com::ServiceIdentifierType LidarServiceInterface::ServiceIdentifier;
constexpr ara::com::ServiceVersionType LidarServiceInterface::ServiceVersion;
}
}

using PcdSkeleton = ara::lidar::skeleton::LidarServiceInterfaceSkeleton;
class PcdSender : public ImageSenderBase<PcdSkeleton> {
public:
    PcdSender() = default;
    PcdSender(uint32_t id, cv::Size s, std::string dir) : ImageSenderBase<PcdSkeleton>(id, s, dir)
    {
        sec = std::chrono::high_resolution_clock::now();
    }
    ~PcdSender() = default;
    bool RegisterSender()
    {
        std::string idStr = std::to_string(instanceId);
        ara::core::StringView id(idStr.c_str());
        dataSkeleton = std::unique_ptr<PcdSkeleton>(
           new PcdSkeleton(ara::com::InstanceIdentifier(id), ara::com::MethodCallProcessingMode::kPoll));
        if (1 == SwitchFlag) {
            dataSkeleton = nullptr;
        }
        if (dataSkeleton == nullptr) {
            std::cerr << "Publish to instance id:" << idStr << " failed!" << std::endl;
            return false;
        }
        dataSkeleton->OfferService();
        return true;
    }

    int retPcdSender = -1;
    void FillImageAndSend(std::string imgPath, uint32_t seq, int32_t *flag) override
    {
        std::string path = dir;
        std::string postfix = g_comPara.fileType;
        bool isLoop = g_comPara.loop;
        std::cout << "dir : " << dir << "\t"
            << "postfix :" << postfix << std::endl;
        std::vector<std::string> pcd_names {};
        if (path.back() != '/') {
            path = path + std::string("/");
        }
        GetFilesFromDirectory(path, pcd_names);
        std::cout << "There are " << pcd_names.size() << " pcd files in " << path << std::endl;
        do {
            InvokingFillImage(path, pcd_names);
        } while (isLoop);
        retPcdSender = 0;
    }

private:
    std::chrono::high_resolution_clock::time_point sec {}; // -t used
    void GetFilesFromDirectory(const std::string &name, std::vector<std::string> &v)
    {
        DIR *dirp = opendir(name.c_str());
        if (dirp == nullptr) {
            retPcdSender = -1;
            return;
        }
        struct dirent *dp;
        while ((dp = readdir(dirp)) != nullptr) {
            if (strcmp(dp->d_name, ".") == 0 || strcmp(dp->d_name, "..") == 0) {
                continue;
            }
            if (EndsWith(dp->d_name, ".pcd")) {
                v.push_back(dp->d_name);
            }
        }
        closedir(dirp);
        retPcdSender = 0;
    }

    bool EndsWith(const std::string &str, const std::string &suffix)
    {
        return str.size() >= suffix.size() && str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;
    }

    void GetLidarDataPoints(pcl::PointCloud<pcl::PointXYZI>::Ptr lidarData, uint8_t *pointCloud0, int32_t loopCount)
    {
        uint8_t *pointCloudCurr = nullptr;
        for (int32_t pointCount = 0; pointCount < loopCount; pointCount++) {
            pointCloudCurr = pointCloud0 + CI13 * pointCount;
            if (g_comPara.tansMode == "simple") {
                *(reinterpret_cast<int16_t *>(pointCloudCurr + X_IN_POINTCLOUD_SIMPLE)) =
                    lidarData->points[pointCount].x * CI100;
                *(reinterpret_cast<int16_t *>(pointCloudCurr + Y_IN_POINTCLOUD_SIMPLE)) =
                    lidarData->points[pointCount].y * CI100;
                *(reinterpret_cast<int16_t *>(pointCloudCurr + Z_IN_POINTCLOUD_SIMPLE)) =
                    lidarData->points[pointCount].z * CI100;
                *(reinterpret_cast<uint8_t *>(pointCloudCurr + INTENSITY_IN_POINTCLOUD_SIMPLE)) =
                    lidarData->points[pointCount].intensity;
            } else {
                *(reinterpret_cast<float *>(pointCloudCurr + X_IN_POINTCLOUD)) = lidarData->points[pointCount].x;
                *(reinterpret_cast<float *>(pointCloudCurr + Y_IN_POINTCLOUD)) = lidarData->points[pointCount].y;
                *(reinterpret_cast<float *>(pointCloudCurr + Z_IN_POINTCLOUD)) = lidarData->points[pointCount].z;
                *(reinterpret_cast<uint16_t *>(pointCloudCurr + INTENSITY_IN_POINTCLOUD)) =
                    lidarData->points[pointCount].intensity;
            }
        }
    }

    void HandleFrequency()
    {
        const int32_t sleepTimeIntervalMs = 1000000;
        if (g_comPara.frequency != "") {
            int32_t freq = atoi(g_comPara.frequency.c_str());
            if (freq != 0) {
                int32_t sleepTime = sleepTimeIntervalMs / freq;
                usleep(sleepTime);
            }
        } else {
            usleep(sleepTimeIntervalMs / 30); // 30hz
        }
    }

    void InvokingFillImage(std::string path, std::vector<std::string> pcd_names)
    {
        for (size_t i = 0; i < pcd_names.size(); i++) {
            const std::string pcd_name = pcd_names[i];
            const std::string pcd_source_path = path + pcd_name;
            std::cout << "The " << i << " pcd source path is: " << pcd_source_path << std::endl;
            pcl::PointCloud<pcl::PointXYZI>::Ptr lidarData(new pcl::PointCloud<pcl::PointXYZI>);
            if (pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_source_path, *lidarData) == -1) {
                std::cout << "load pcd failed" << std::endl;
                continue;
            }
            std::cout << "Load local pcd successful, pointcloud size is: " <<
                lidarData->width << ", " << lidarData->height << std::endl;
            std::stringstream str;
            str << "copy-" << pcd_name;
#ifdef SAVE_SENDER_FILE
            pcl::PCDWriter writer;
            writer.writeBinaryCompressed<pcl::PointXYZI>(str.str(), *lidarData);
#endif
            ara::lidar::LidarPointCloud tmp;
            tmp.header.frameId = pcd_name;
            if (g_comPara.tansMode == "simple") {
                // 13表示简易点云结构大小
                tmp.pointStep = 13;
            } else {
                // 32表示标准点云结构大小
                tmp.pointStep = 32;
            }
            bool result = DealTmp(lidarData, tmp, pcd_name, i);
            if (!result) {
                return;
            }
        }
    }

    bool DealTmp(pcl::PointCloud<pcl::PointXYZI>::Ptr lidarData, ara::lidar::LidarPointCloud &tmp,
        const std::string pcd_name, size_t i)
    {
        tmp.height = lidarData->height;
        tmp.width = lidarData->width;
        std::vector<uint8_t> pointCloudData;
        pointCloudData.resize(tmp.pointStep * tmp.height * tmp.width);
        uint8_t *pointCloud0 = &pointCloudData[0];
        uint8_t *pointCloudCur = nullptr;
        int32_t loopCount = static_cast<int32_t>(tmp.height) * static_cast<int32_t>(tmp.width);
        const int32_t sliceSize = 7400000;
        int32_t maxNumOfPoints = (sliceSize - sizeof(tmp.header)) / tmp.pointStep;
        if (1 == SwitchFlag) {
            loopCount = maxNumOfPoints + 1;
        }
        if (loopCount > maxNumOfPoints) {
            std::cout << pcd_name << " send failed due to too many points,max points num is " <<
                maxNumOfPoints << std::endl;
            return false;
        }
        GetLidarDataPoints(lidarData, pointCloud0, loopCount);
        tmp.data.assign(pointCloudData.begin(), pointCloudData.end());
        if (g_comPara.time.empty()) {
            timeval tv;
            gettimeofday(&tv, 0);
            tmp.header.stamp.sec = tv.tv_sec;
            tmp.header.stamp.nsec = tv.tv_usec = 1e3;
        } else {
            time_t timeStamp = ConvertTimeStr2TimeStamp(g_comPara.time);
            auto t1 = std::chrono::high_resolution_clock::now();
            int64_t duration = (t1 - sec).count() / 1000000000.0;
            tmp.header.stamp.sec = timeStamp + duration;
            printf("timeStamp=%ld\n", timeStamp + duration);
        }
        tmp.header.seq = i;
        std::cout << "tmp.header.stamp.sec: " << tmp.header.stamp.sec << std::endl;
        std::cout << "tmp.header.stamp.nsec: " << tmp.header.stamp.nsec << std::endl;
        std::cout << "tmp.header.frameId: " << tmp.header.frameId << std::endl;
        dataSkeleton->mdcEvent.Send(std::move(tmp));
        const int32_t sleepTimeIntervalMs = 1000000;
        HandleFrequency();
        return true;
    }
};

#endif // IMAGE_SENDER_PCD_SENDER_H
